#!/usr/bin/env python3
# -*- coding: utf-8 -*-
from ArmMove import ArmMover
import ArmControl
import rospy, sys
from geometry_msgs.msg import PoseStamped, Pose
import moveit_commander
from moveit_commander import MoveGroupCommander
from copy import deepcopy
import GripperControl
from time import sleep
from math import pi

 # 角度转弧度
DE2RA = pi / 180.0
# 弧度转角度
RA2DE = 180.0 / pi

# grap为设置正向 loose为设置反向 每发送一次 均为单次执行 
# 若夹红枣 只需单次 杯子需要双次 
gripper_grap = b'\x01\x06\x00\x0b\x00\x01\x39\xc8'
gripper_loose = b'\x01\x06\x00\x0b\x00\x00\xf8\x08'


# 姿态使用四元数描述，基于base_link坐标系
pos = [-0.048690970477408904,0.39523770914892736,0.2896239715352084]
ori = [0.19854933376993117, -0.9800405916192696,0.009086124848287748, 0.004005393028150726]
target_pose = Pose()

# target_pose.orientation.x = q[0]
# target_pose.orientation.y = q[1]
# target_pose.orientation.z = q[2]
# target_pose.orientation.w = q[3]
# target_pose.position.x = p[1]
# target_pose.position.y = 
# target_pose.position.z = -0.008051004234953705

interval_num = 3
grap_num = 0    

joints_0 = [55.23161185043341, -58.082498760842306, 40.164545583462, -80.29589581317823, -31.46440591291913, 110.26606324619873]


# 接受到的数据信息
msg = [0,0,0,0]
name = "beizi"

def InitPose(pos,ori):
    '''
    初始化Pose
    :param pos :装xyz的列表
    :param ori :装xyzw的列表
    '''
    poses = Pose()
    poses.position.x = pos[0]
    poses.position.y = pos[1]
    poses.position.z = pos[2]
    poses.orientation.x = ori[0]
    poses.orientation.y = ori[1]
    poses.orientation.z = ori[2]
    poses.orientation.w = ori[3]
    return poses

def OpenBox(name):
    '''
    固定位置开箱 
    初始 夹爪1松 结束 1松
    :param name:材料名称 根据name的不同选择joints0 joints1 jointsbox0 jointsbox1
    '''
    # 机械臂初始化
    Con = ArmMover()
    # 夹爪初始化
   
    # 调节速度标志位 id16有点区别
    end_flag = 0
    if id == 1:
        joints_look = [76.94363208733526, 16.29648428658548, 99.91687328854839, 12.224516750904282, 84.27566382307032, 86.06979785994304]
        # 根据name可调
        joints_box_0 = [28.132997977398265, -13.243436618777928, 75.25728484574512, -99.27203664242067, -154.29570231352997, 86.85884196326462]
        joints_box_1 = [28.132997977398265, -13.243436618777928, 75.25728484574512, -99.27203664242067, -154.29570231352997, 86.85884196326462]
        # 拉取扶手预备位置
        joints_0 = [56.590881674470204, -44.92881246750753, 82.90822069946535, -52.099563246724806, -125.71152727868652, 94.14940920861707]
        joints_1 = [32.6993417015198, -31.80866559443761, 97.03271057933273, -59.13049797907581, -145.57689910388908, 91.85197217540689]
        # 拉取起点终点
        joints_start = [61.563418230415756, -53.247733893805574, 61.513639467424944, -67.83100058748904, -116.6186983955697, 97.26690554736712]
        joints_end = [46.11548662991096, -35.94496838697015, 100.4647431328895, -46.74830970525101, -132.04700747826058, 96.40551940830395]
        end_flag = 1

    elif id == 2:
        joints_look = [39.05303894789994, -14.983076142880083, 32.36152486437031, -25.426913268924714, 96.29906817407995, 105.96071743524513]
        # 根据name可调
        joints_box_0 = [90.96976184635326, 1.4461970826575847, 98.99639997539316, 85.80525190352866, 4.0302822386527, 62.53982135113543]
        joints_box_1 = [90.96976184635326, 1.4461970826575847, 98.99639997539316, 85.80525190352866, 4.0302822386527, 62.53982135113543]
        # 拉取扶手预备位置
        joints_0 = [65.60503429436376, -40.89343686096325, 85.24905017589215, -61.299409393965014, -26.385626055509775, 111.60483811239148]
        joints_1 = [110.23560873365858, -28.74082501286636, 123.70857627525442, -17.2704643044231, 18.726867440812892, 95.03193135133657]
        # 拉取起点终点
        joints_start = [60.32465078155394, -53.46311351002266, 71.11588733499923, -61.702870187196154, -31.636153194068683, 110.29157048270565]
        joints_end = [87.1680434782245, -44.33987558972909, 99.4690836512773, -71.5504869670366, -5.689869046737791, 140.02781773411687]

    elif id == 3:
        joints_look = [36.26718797748869, -31.787686072887187, 5.32424205197828, -28.732074531265887, 91.19551567882215, 104.10368125339096]
        # 根据name可调
        joints_box_0 = [90.96976184635326, 1.4461970826575847, 98.99639997539316, 85.80525190352866, 4.0302822386527, 62.53982135113543]
        joints_box_1 = [90.96976184635326, 1.4461970826575847, 98.99639997539316, 85.80525190352866, 4.0302822386527, 62.53982135113543]
        # 拉取扶手预备位置
        joints_0 = [57.502406947854816, -43.69760870852455, 57.55575834314929, -77.110554445484, -29.138383301257914, 124.63921136276045]
        joints_1 = [102.0676946132626, -17.717198032046333, 112.00082070269704, -36.35335252349134, 14.16517898991094, 97.95021174997856]
        # 拉取起点终点
        joints_start = [55.23161185043341, -58.082498760842306, 40.164545583462, -80.29589581317823, -31.46440591291913, 110.26606324619873]
        joints_end = [79.70375290731371, -35.472140625406155, 88.07529469342802, -78.0437348636222, -9.24472408134642, 132.84738089296934]
    
    elif id == 4:
        joints_look = [-70.55285454033132, -2.5391522153567463, -110.77921508948857, -48.18797398110102, -101.18574282329703, 77.18020855393655]
        # 根据name可调
        joints_box_0 = [-41.6021998012661, -5.501912950327209, -113.73862596038822, 70.72861725271528, 140.3482737292506, 108.71981884333651]
        joints_box_1 = [-41.6021998012661, -5.501912950327209, -113.73862596038822, 70.72861725271528, 140.3482737292506, 108.71981884333651]
        # 拉取扶手预备位置
        joints_0 = [-62.90600707702462, 47.31760763394238, -87.53396133602335, 44.412134792058545, 119.01443985593963, 109.09697398770953]
        joints_1 = [-38.270805530005966, 40.16069835543829, -112.96788458925259, 25.721944465427725, 143.6495378910664, 108.57134966760852]
        # 拉取起点终点
        joints_start = [-67.8346215551143, 56.29933858322614, -67.53900197048821, 55.46338592896067, 114.08562715018326, 109.17533285268979]
        joints_end = [-48.14177897793145, 41.90778622280469, -108.02132068976863, 29.13281504073732, 133.77832950868083, 108.83679680504076]

    elif id == 5:
        joints_look = [-42.57587155686836, 43.636082810308984, 31.21475449443847, 68.73572397043739, -90.71708906366561, 99.1708766628771]
        # 根据name可调
        joints_box_0 = [-91.78812753218035, -21.005125757969072, -84.63222165000006, 4.033883594788217, 5.8146355303261235, 68.31719458715988]
        joints_box_1 = [-91.78812753218035, -21.005125757969072, -84.63222165000006, 4.033883594788217, 5.8146355303261235, 68.31719458715988]
        # 拉取扶手预备位置
        joints_0 = [-65.01962202592907, 46.01214881768056, -92.06981961063104, 36.77555309860527, 25.503722197863617, 91.34759051154472]
        joints_1 = [-104.5689706772471, 43.21892419848777, -123.121895884477, 22.430115450100843, -14.311863396685643, 91.6040618004917]
        # 拉取起点终点
        joints_start = [-57.693525303904856, 58.00858852761728, -76.30515499931741, 41.59696292784344, 32.79744782948376, 103.45996160747508]
        joints_end = [-87.66880580643188, 51.00769919373861, -133.0750793029898, -42.638140131345935, 3.483046884614006, 138.58190447418568]

    elif id == 6:
        joints_look = [-41.84353150979235, 35.19294870307727, 19.52862003461151, 50.14991325089466, -93.44378135070984, 99.31342439621388]
        # 根据name可调
        joints_box_0 = [-91.78812753218035, -21.005125757969072, -84.63222165000006, 4.033883594788217, 5.8146355303261235, 68.31719458715988]
        joints_box_1 = [-91.78812753218035, -21.005125757969072, -84.63222165000006, 4.033883594788217, 5.8146355303261235, 68.31719458715988]
        # 拉取扶手预备位置
        joints_0 = [-56.91458906636432, 43.269322531743754, -65.99910954919467, 66.7626140533892, 33.607159574871375, 103.34614120558263]
        joints_1 = [-106.18046392844973, 23.597296328699663, -124.52960013641807, 39.62823938408572, -15.8613837633491, 92.63958357452978]
        # 拉取起点终点
        joints_start = [-50.49179502429021, 57.01038810851468, -47.65471464567712, 71.90255750430973, 40.054139520512265, 102.61840554823924]
        joints_end =  [-77.34095029193261, 26.987877898310185, -112.07590398663903, 31.587163497168913, 13.313769904957146, 109.08779343393525]
        end_flag = 1
        # [-81.63167769591732, 25.558777689896466, -110.06989137905025, 45.03472421933769, 8.444006976340106, 99.43297754924053]  # 0.1可

    # 拉取初始
    Con.MovebyJoint(joints_box_0,0.3)
    # 抓取预备起点
    Con.MovebyJoint(joints_0,0.3)
    # 起点
    Con.MovebyJoint(joints_start,0.3)
    # 夹爪加紧
    GripperControl.newcontrol(1)
    sleep(0.5)
    # 终点
    Con.MovebyJoint(joints_end,0.3 - end_flag*0.2)
    # 夹爪松
    GripperControl.newcontrol(0)
    # 后拉至关节预备终点点
    Con.MovebyJoint(joints_1,0.3)
    # 退回初始位置
    Con.MovebyJoint(joints_box_1,0.3)
    # 到达下一识别位置
    Con.MovebyJoint(joints_look,0.3)
    # # 夹爪初始化【抓取材料】
    # GripperControl.newcontrol(2)

def CloseBox(name):
    '''
    固定位置关箱 
    初始 夹爪0松 结束 1松
    :param name:材料名称 根据name的不同选择joints0 joints1 jointsbox0 jointsbox1
    内值均与openbox反向 完全对照
    '''
    # 机械臂初始化
    Con = ArmMover()

    if id == 1:
        joints_box_0 = [28.132997977398265, -13.243436618777928, 75.25728484574512, -99.27203664242067, -154.29570231352997, 86.85884196326462]
        joints_box_1 = [28.132997977398265, -13.243436618777928, 75.25728484574512, -99.27203664242067, -154.29570231352997, 86.85884196326462]
        # 拉取扶手预备位置
        joints_0 = [56.590881674470204, -44.92881246750753, 82.90822069946535, -52.099563246724806, -125.71152727868652, 94.14940920861707]
        joints_1 = [32.6993417015198, -31.80866559443761, 97.03271057933273, -59.13049797907581, -145.57689910388908, 91.85197217540689]
        # 拉取起点终点
        joints_start = [61.563418230415756, -53.247733893805574, 61.513639467424944, -67.83100058748904, -116.6186983955697, 97.26690554736712]
        joints_end = [42.68577722457199, -33.19330083276743, 99.25756889752329, -54.011923368230555, -135.65559061259034, 93.82608241452083]

    elif id == 2:
        joints_box_0 = [90.96976184635326, 1.4461970826575847, 98.99639997539316, 85.80525190352866, 4.0302822386527, 62.53982135113543]
        joints_box_1 = [90.96976184635326, 1.4461970826575847, 98.99639997539316, 85.80525190352866, 4.0302822386527, 62.53982135113543]
        # 拉取扶手预备位置
        joints_0 = [65.60503429436376, -40.89343686096325, 85.24905017589215, -61.299409393965014, -26.385626055509775, 111.60483811239148]
        joints_1 = [110.23560873365858, -28.74082501286636, 123.70857627525442, -17.2704643044231, 18.726867440812892, 95.03193135133657]
        # 拉取起点终点
        joints_start = [56.62008650943562, -58.85807931190627, 59.98336372232351, -66.83899505936205, -35.25078594696216, 109.62641860163437]
        joints_end = [87.1680434782245, -44.33987558972909, 99.4690836512773, -71.5504869670366, -5.689869046737791, 140.02781773411687]

    elif id == 3:
        # 根据name可调
        joints_box_0 = [90.96976184635326, 1.4461970826575847, 98.99639997539316, 85.80525190352866, 4.0302822386527, 62.53982135113543]
        joints_box_1 = [90.96976184635326, 1.4461970826575847, 98.99639997539316, 85.80525190352866, 4.0302822386527, 62.53982135113543]
        # 拉取扶手预备位置
        joints_0 = [57.502406947854816, -43.69760870852455, 57.55575834314929, -77.110554445484, -29.138383301257914, 124.63921136276045]
        joints_1 = [102.0676946132626, -17.717198032046333, 112.00082070269704, -36.35335252349134, 14.16517898991094, 97.95021174997856]
        # 拉取起点终点
        joints_start = [51.64692142796036, -72.91810038872438, 9.217116806523073, -96.46602758927037, -34.99270235519795, 110.40568848086932]
        joints_end = [79.70375290731371, -35.472140625406155, 88.07529469342802, -78.0437348636222, -9.24472408134642, 132.84738089296934]

    elif id == 4:
        # 根据name可调
        joints_box_0 = [-41.6021998012661, -5.501912950327209, -113.73862596038822, 70.72861725271528, 140.3482737292506, 108.71981884333651]
        joints_box_1 = [-41.6021998012661, -5.501912950327209, -113.73862596038822, 70.72861725271528, 140.3482737292506, 108.71981884333651]
        # 拉取扶手预备位置
        joints_0 = [-62.90600707702462, 47.31760763394238, -87.53396133602335, 44.412134792058545, 119.01443985593963, 109.09697398770953]
        joints_1 = [-38.270805530005966, 40.16069835543829, -112.96788458925259, 25.721944465427725, 143.6495378910664, 108.57134966760852]
        # 拉取起点终点
        joints_start = [-67.8346215551143, 56.29933858322614, -67.53900197048821, 55.46338592896067, 114.08562715018326, 109.17533285268979]
        joints_end = [-48.14177897793145, 41.90778622280469, -108.02132068976863, 29.13281504073732, 133.77832950868083, 108.83679680504076]

    elif id == 5:
        # 根据name可调
        joints_box_0 = [-91.78812753218035, -21.005125757969072, -84.63222165000006, 4.033883594788217, 5.8146355303261235, 68.31719458715988]
        joints_box_1 = [-91.78812753218035, -21.005125757969072, -84.63222165000006, 4.033883594788217, 5.8146355303261235, 68.31719458715988]
        # 拉取扶手预备位置
        joints_0 = [-65.01962202592907, 46.01214881768056, -92.06981961063104, 36.77555309860527, 25.503722197863617, 91.34759051154472]
        joints_1 = [-104.5689706772471, 43.21892419848777, -123.121895884477, 22.430115450100843, -14.311863396685643, 91.6040618004917]
        # 拉取起点终点
        joints_start = [-57.693525303904856, 58.00858852761728, -76.30515499931741, 41.59696292784344, 32.79744782948376, 103.45996160747508]
        joints_end = [-87.66880580643188, 51.00769919373861, -133.0750793029898, -42.638140131345935, 3.483046884614006, 138.58190447418568]
    
    elif id == 6:
        # 根据name可调
        joints_box_0 = [-91.78812753218035, -21.005125757969072, -84.63222165000006, 4.033883594788217, 5.8146355303261235, 68.31719458715988]
        joints_box_1 = [-91.78812753218035, -21.005125757969072, -84.63222165000006, 4.033883594788217, 5.8146355303261235, 68.31719458715988]
        # 拉取扶手预备位置
        joints_0 = [-56.91458906636432, 43.269322531743754, -65.99910954919467, 66.7626140533892, 33.607159574871375, 103.34614120558263]
        joints_1 = [-106.18046392844973, 23.597296328699663, -124.52960013641807, 39.62823938408572, -15.8613837633491, 92.63958357452978]
        # 拉取起点终点
        joints_start = [-50.49179502429021, 57.01038810851468, -47.65471464567712, 71.90255750430973, 40.054139520512265, 102.61840554823924]
        joints_end =  [-77.34095029193261, 26.987877898310185, -112.07590398663903, 31.587163497168913, 13.313769904957146, 109.08779343393525]
        
    # 拉取初始
    Con.MovebyJoint(joints_box_1,0.3)
    # 抓取预备起点
    Con.MovebyJoint(joints_1,0.3)
    # 起点
    Con.MovebyJoint(joints_end,0.3)
    # 夹爪加紧
    GripperControl.newcontrol(1)
    sleep(0.5)
    # 终点
    Con.MovebyJoint(joints_start,0.3)
    # 夹爪松
    GripperControl.newcontrol(0)
    # 后拉至关节预备终点点
    Con.MovebyJoint(joints_0,0.3)
    # 退回初始位置
    Con.MovebyJoint(joints_box_0,0.3)

def video_grap():
    '''
    夹爪 初始1松 结束1松
    :param joints: 材料起点
    :param joints_down: 材料终点 固定即可
    :param name: 材料名称 根据name的不同选择jointslook joints_cailiao_0

    '''
    Arm = ArmMover()

    # 抬起
    joints_look = [73.10607148938311, -8.246709720293916, 51.27173137607049, -28.462294467562934, 84.60720161376365, 67.83694708621478]
    joints_look_all = [20.37368914274531, -21.55171909497705, 32.87061483033298, -35.563209627034915, 89.92346654814281, 49.20774757198476]
    # 设定的位置
    joints = [46.96208199761266, -57.13540642984541, 51.699467139069974, 20.18107483726318, 83.8151932235901, 72.72693778419547]
    joints_cailiao_0 =  [72.59305397722844, -4.046071896997263, 47.894406627391064, -37.22328198881641, 88.04016661464863, 87.8169316950762]
    joints_down = [-1.6187671977891651, -38.08107011574237, 3.0086032451056637, -43.43213859732666, 95.63002275877926, 77.46779396184895]
    
    # 移动到抽屉位置上方
    Arm.MovebyJoint(joints_look,0.3)  

    # 移动至物体位置7
    Arm.MovebyJoint(joints,0.3)

    # # 进行抓取,夹紧夹爪
    GripperControl.newcontrol(2)
    sleep(0.5)

    # 抬起至对应位置上方
    Arm.MovebyJoint(joints_cailiao_0,0.3)

    # 抬起至对应位置
    Arm.MovebyJoint(joints_down,0.3)

    # # 释放物体,松开夹爪
    GripperControl.newcontrol(0)

    # 抬起
    Arm.MovebyJoint(joints_look_all,0.3)
    sleep(0.5)
 
def beizi_Grap():
    '''
    移动过程
    夹爪初始 2松 结束 2松
    :param joints: 杯子起点
    :param joints_down: 杯子终点 固定即可
    '''
    # 初始化机械臂
    Arm = ArmMover()
    # 初始化夹爪
    GripperControl.newcontrol(0)
    sleep(0.5)

    # 初始位置
    joints_look = [-60.67638917551703, 30.958930588437717, 66.30949807520462, -24.253970457265563, 88.6308377313422, 143.5199429315394]
    # 识别杯子
    joints = [-80.07528142130465, -38.889643942294164, 50.96443293163518, -1.1279748242886884, 90.0850916578812, 32.87494365342534]   #change
    joints_down =[-4.046883011209535, -24.360175419897672, 51.41464726565747, -14.200682276065276, 94.50595899515324, 74.57322971675787]  #change
    # 抬起
    joints_beizi_0 = [-1.6187671977891651, -38.08107011574237, 3.0086032451056637, -43.43213859732666, 95.63002275877926, 77.46779396184895] #change
    joints_up = [20.37368914274531, -21.55171909497705, 32.87061483033298, -35.563209627034915, 89.92346654814281, 49.20774757198476]
    
    # 移动至物体位置上方
    Arm.MovebyJoint(joints_look)
    # 移动至物体位置
    Arm.MovebyJoint(joints)
    # 进行抓取,夹紧夹爪
    GripperControl.newcontrol(1)
    sleep(0.5)

    # 抬起至对应位置上方
    j =  [joints_down[0],-32, 16, -42, 90, 23]
    Arm.MovebyJoint(j,0.5)

    # 抬起至对应位置
    Arm.MovebyJoint(joints_down)

    # # 释放物体,松开夹爪
    GripperControl.newcontrol(0)
    sleep(0.5)

    # 抬起
    Arm.MovebyJoint(joints_beizi_0)

    #归中 接开箱操作
    Arm.MovebyJoint(joints_up,0.5)
  
def Beizi_To_Drink():
    
    print("now:moving beizi to drink  !")

    # 初始化机械臂
    Arm = ArmMover()
     # 初始化夹爪
    GripperControl.newcontrol(0)

    # 初始位置
    joints_look_0 = [45.24030863930541, -21.669151413962254, 10.56932921605124, -54.49550842369311, 94.62258883592305, 57.10411569307362]
    # 为下一动作【按压饮水机Push_Drink】的初始化动作
    joints_look_1 = [-18.35721598561729, 10.101406014558284, -25.763873223856027, 42.88951983194061, -89.14777385579383, 60.42179978284899]

    
    # 杯子预备位置
    joints_drink_0 = [ 42.85494107715227, -50.97346718235573, 50.196320602967646, -74.5048198757327, 42.793337369584286, 41.76684312382531]
    joints_drink_1 = [-79.24493798583275, -44.78447626588668, 73.85034967459715, -52.45126277096409, 46.236071543499854, 64.3282291367248]

    # 抓杯子过程位置
    joints_0 = [32.997183351479784, -43.416156667305685, 65.77677569149917, -65.37759470196129, 32.95781598689861, 40.381520479285065]
    # 略抬升位置
    joints_1 = [31.535846991877722, -52.32468393495886, 40.38652396837164, -81.49217376010192, 31.51293423592395, 46.06208915547212]
    # 旋转接近饮水机处
    joints_2 = [-87.3172889523259, -52.32142238152142, 40.3832476394609, -81.47707777231993, 39.855501570532404, 46.045216885501205]
    # 到达饮水机位置
    joints_3 = [-93.7617377557454, -31.82924466191599, 84.99398632981274, -56.37260399800603, 33.45098014696846, 44.82606361756877]

    
    # # 移动至物体位置上方
    Arm.MovebyJoint(joints_look_0,0.5)

    # 预备抓杯位置
    Arm.MovebyJoint(joints_drink_0,0.3)

    # 抓取位置
    Arm.MovebyJoint(joints_0,0.5)
    # 夹爪抓取
    GripperControl.newcontrol(1)
    sleep(0.5)
    # 略抬升位置
    Arm.MovebyJoint(joints_1,0.5)
    # 旋转接近饮水机处
    Arm.MovebyJoint(joints_2,0.2)  
    # 到达饮水机位置
    Arm.MovebyJoint(joints_3,0.5)
    # 夹爪松开
    GripperControl.newcontrol(0)
    sleep(0.5)
    # 后退位置
    Arm.MovebyJoint(joints_drink_1,0.5)

    # 抬起 为下一动作【按压饮水机Push_Drink】的初始化动作
    Arm.MovebyJoint(joints_look_1,0.2)

def Push_Drink():
    
    # 初始化机械臂
    Arm = ArmMover()

    # 初始化夹爪
    GripperControl.newcontrol(0)

    # 初始位置
    joints_look = [-90,-32, 16, -42, 90, 23]
    # 识别杯子
    joints_drink_0 = [-18.35721598561729, 10.101406014558284, -25.763873223856027, 42.88951983194061, -89.14777385579383, 60.42179978284899]
    joints_drink_1 = [-18.35721598561729, 10.101406014558284, -25.763873223856027, 42.88951983194061, -89.14777385579383, 60.42179978284899]

    joints_0 = [7.157536648054807, 13.767147463301283, -44.37016393507476, 12.015213169062784, -92.36542603572808, 66.88363090726718]
    joints_1 = [5.563633072561074, 6.727235361772198, -62.63084012504152, 0.7412671800391712, -91.7935740910617, 65.32483780326977]
    joints_11= [7.157536648054807, 13.767147463301283, -44.37016393507476, 12.015213169062784, -92.36542603572808, 66.88363090726718]
    joints_2 = [7.497770858180807, 35.18360864906135, -5.62761775526084, 32.48033024637178, -95.94788706557503, 106.0901352166828]
    joints_3 = [9.23782476789352, 21.660501086496307, -37.178721556100726, 17.696449530959356, -95.47348073434101, 108.10082990233698]
    joints_4 = [8.895796794099375, 29.82939167216471, -12.369404252401708, 31.219684002078164, -96.35160938828012, 107.43711647936308]

    
    # # 移动至物体位置上方
    # Arm.MovebyJoint(joints_look)

    # # 初始化
    # Arm.MovebyJoint(joints_drink_0,0.5)

    # 饮水机童锁上方
    Arm.MovebyJoint(joints_0,0.5)
    # 饮水机按钮按童锁
    Arm.MovebyJoint(joints_1,0.5)
    sleep(1)
    Arm.MovebyJoint(joints_11,0.5)
    sleep(1)
    # 饮水机开关上方
    Arm.MovebyJoint(joints_2,0.5)  
    # 饮水机按钮按250ml并开启
    Arm.MovebyJoint(joints_3,0.5)
    # 饮水机上方
    Arm.MovebyJoint(joints_4,0.5)
    # 抬起
    Arm.MovebyJoint(joints_drink_1,0.5)

def Get_Drink():
    # 初始化机械臂
    Arm = ArmMover()
    # 初始化夹爪
    GripperControl.newcontrol(0)
    sleep(0.5)

    # 初始位置
    joints_look = [-90,-32, 16, -42, 90, 23]
    # 杯子预备位置
    joints_drink_0 = [-80.41020607088991, -34.8470549319062, 74.18243584578649, -61.832637144726654, 45.022171536507656, 20.512724040675966]
    joints_drink_1 = [-90,-32, 16, -42, 90, 23]

    joints_0 = [-92.7577702283026, -30.702823986508566, 88.30935886238296, -49.02629049397884, 32.89250812404119, 28.075834772565035]
    joints_1 = [-80.41020607088991, -34.8470549319062, 74.18243584578649, -61.832637144726654, 45.022171536507656, 20.512724040675966]
    joints_3 = [-7.974800958608117, -0.4540214624496332, 122.59586252592715, -54.91805498002628, -97.28796424190058, 28.925238904347932]
    joints_2 =  [-9.077455980620101, -8.274095165323153, 127.31495044430211, -42.36492435502244, -98.38927518926192, 28.969842332936764]
    joints_4 = [-30.383106264072968, 0.41192952217100487, 133.08773590164736, -44.954040094452075, -119.67251862965907, 29.843977111342003]

    # 抓杯子预备位置
    Arm.MovebyJoint(joints_drink_0,0.5)
    # 抓住杯子位置
    Arm.MovebyJoint(joints_0,0.5)
    # 夹爪抓取
    GripperControl.newcontrol(2)
    sleep(0.5)
    # 把杯子移动出来避免碰撞
    Arm.MovebyJoint(joints_1,0.5)  
    # 移动杯子到用户区
    Arm.MovebyJoint(joints_3,0.5)
    # 下降
    Arm.MovebyJoint(joints_2,0.5)
    # 松开杯子
    GripperControl.newcontrol(0)
    sleep(0.5)
    # 后撤避免碰撞
    Arm.MovebyJoint(joints_4,0.5)
    # 初始化位置
    Arm.MovebyJoint(joints_drink_1,0.5)

if __name__ == "__main__":
    
    # # Con.MovebyPose(start_pose)
    # Con.ShiftPose(0, -0.05,0.1)

    # 获取角度
    # Con = ArmMover()
    # state = Con.arm.get_current_joint_values()
    # for i in range(6):
    #     state[i] = state[i] * RA2DE
    # print(state)

    # # # 位置测试
    # Con = ArmMover()
    # Con.MovebyJoint(joints_0,0.1)
    # GripperControl.newcontrol(1)


    #获取末端姿态
    # Con = ArmMover()
    # state = Con.arm.get_current_pose()
    # print(state)
    # InitPose(pos, ori)
    
    # GripperControl.newcontrol(0)
    # GripperControl.newcontrol(2)

    global id
    id = 6
    while True:
        # OpenBox(name)
        # CloseBox(name)
        beizi_Grap()
        Beizi_To_Drink()
        Push_Drink()
        Get_Drink()
    


        
        

    # GripperControl.newcontrol(0)
    # sleep(2)
    # Beizi_To_Drink()
    # Get_Drink()



    # 位置测试
    # Con = ArmMover()
    # Con.MovebyPose(target_pose,0.1)
    # Con.ShiftPose(0, -0.15,0.1)

    # 杯子夹取+红枣夹取
    # beizi_Grap()
    # GripperControl.newcontrol(1)
    # sleep(0.5)
    # video_grap()

    ## Arm测试

    # 初始化测试
    # ArmControl.Init_State()

    # 杯子抓取测试 有一定概率轨迹不好
    # pos = [-0.048721126668757025, 0.3949599739271275,0.2902972867371111]
    # ori = [ 0.1978820212335268, -0.980185988127384, 0.008052939010414631, 0.003644794174240588]
    # target_pose = InitPose(pos,ori)
    # joints = [-6.058711977745116, -24.77507630274375, 84.84376830880049, 19.52928183022381, 90.00086604464862, 17.57600009825649]
    # ArmControl.Beizi_Grap(target_pose, joints)

    # # 开箱取物测试 
    # ArmControl.Open_Box('jiangtang')
    # pos = [ 0.36360111543829393, -0.22539549595379393, 0.2106817009523401 ]
    # ori = [ 0.5310942076273214, 0.8454578435374113, 0.049674669719567265, 0.025930765781784337 ]
    # target_pose = InitPose(pos,ori)
    # joints = [-1.7610139559094675, -6.975653740697562, 75.27810930194259, -2.1155243694885693, 87.43289882199683, 28.19401302943962]
    # ArmControl.Cailiao_Grap(target_pose, joints,"chenpi")

